#include "AP_ICP.h"

AP_ICP::AP_ICP()
{

 first_exec = true;
 
 opt_match_extent = 10;
 opt_match_threshold = 0.1*0.1;

}

void AP_ICP::scan_polarTocartesian(TypeScan& scan,vector<GPoint2D>& cart_scan_)
{

   float cos_curangle, sin_curangle,cur_angle, dist;
   cart_scan_.resize(scan.nbPoints);
   
     for (int i = 0; i < scan.nbPoints; i++) {
    			
			cur_angle = scan.tabScan[i].angle * M_PI / 180;
			dist = scan.tabScan[i].distance;
			cos_curangle = cos(cur_angle);
			sin_curangle = sin(cur_angle);
			
		  cart_scan_[i].x = dist * cos_curangle;
		  cart_scan_[i].y = -dist * sin_curangle;

		  }

}

void AP_ICP::analyse(TypeScan& scan)
{
    if (AP_DEBUG) cout << "void AP_ICP::analyse" << endl;
    
    int i,k,j; 
    int id_deb,id_end; 
    int nb_pts;
    float temp_min = 2e6;
    float temp;
    int temp_id;
     
     dx_tot = 0;
     dy_tot = 0;
     
      scan_polarTocartesian(scan_prev,cart_scan_prev);
      scan_polarTocartesian(scan,cart_scan);
      scan_polarTocartesian(scan,cart_scan_match);
    
  if(!first_exec)
  {        

    
    for(j=0;j<20;j++)
    {
      
      nb_pts =  cart_scan_prev.size();
      
    if(nb_pts !=  cart_scan.size() )
    {
       cout << "scan size mismatch";
       return;
    }
   
   /****************************************************************************************************/ 
   /* Nearest Neighbor Match */
   /****************************************************************************************************/  
    matched_id_c.clear();
    matched_id_p.clear();
    
    for(i=0;i<nb_pts;i++)
    {
        float x_pt_match = cart_scan_prev[i].x;
        float y_pt_match = cart_scan_prev[i].y;
        
        id_deb =  (i-opt_match_extent>=0)?(i-opt_match_extent):0;
        id_end =  (i+opt_match_extent<nb_pts)?(i+opt_match_extent):nb_pts-1;
         
         temp_min = 2e6;
         
         for(k= id_deb;k<=id_end;k++)
         {
               temp = (x_pt_match-cart_scan_match[k].x)*(x_pt_match-cart_scan_match[k].x) + (y_pt_match-cart_scan_match[k].y)*(y_pt_match-cart_scan_match[k].y);
               
               if(temp <temp_min)
               {
                temp_min = temp;
                temp_id = k;
               } 
         }
         
         if(temp_min < opt_match_threshold)
         {
              matched_id_c.push_back(temp_id);
              matched_id_p.push_back(i);
         
         }

    }
  /****************************************************************************************************/    
    
    if( matched_id_c.size() > 10)
    {
    
        float xcyk_ycxk_sum = 0;
        float xcxk_ycyk_sum = 0;
        int ic,ik;    
        float xc,yc;
        float xk,yk;
        
        for(i=0;i<matched_id_c.size();i++)
        {
           ic =matched_id_c[i];
           ik =matched_id_p[i];
          
           xc =   cart_scan[ic].x;
           yc =   cart_scan[ic].y;
          
           xk =   cart_scan_prev[ik].x;
           yk =   cart_scan_prev[ik].y;
          
           xcyk_ycxk_sum += xc*yk-yc*xk;
           xcxk_ycyk_sum += xc*xk+yc*yk;
        
        }
        
        dtheta = atan(xcyk_ycxk_sum/xcxk_ycyk_sum);
         dtheta = 0;
        dx = 0;
        dy = 0;
        
        for(i=0;i<matched_id_c.size();i++)
        {
           ic =matched_id_c[i];
           ik =matched_id_p[i];
          
           xc =   cart_scan[ic].x;
           yc =   cart_scan[ic].y;
          
           xk =   cart_scan_prev[ik].x;
           yk =   cart_scan_prev[ik].y;
           
           dx += xk - (xc*cos(dtheta)-yc*sin(dtheta));
           dy += yk - (xc*sin(dtheta)+yc*cos(dtheta));
           
        }
        
        dx = dx / ((float)matched_id_c.size());
        dy = dy / ((float)matched_id_c.size());
        
        for(i=0;i<cart_scan_match.size();i++)
        {
            xc =   cart_scan[i].x;
            yc =   cart_scan[i].y;
            cart_scan_match[i].x = (xc*cos(dtheta)-yc*sin(dtheta)) +dx;
            cart_scan_match[i].y =  (xc*sin(dtheta)+yc*cos(dtheta)) + dy;
        }
        
        dx_tot +=   dx;
        dy_tot +=   dy;
        
 //       cout<<"dtms:"<<dtms<<"  nb_matched:"<<matched_id_c.size()<<endl;
        cout<<"dx:"<<dx<<"  dy:"<<dy<<"  dtheta:"<<dtheta<<endl;
    
    }
    
    }
   
	}
      
    
	for (int i = 0; i < scan.nbPoints; i++) {
		scan_prev.tabScan[i].distance = scan.tabScan[i].distance;
		scan_prev.tabScan[i].angle = scan.tabScan[i].angle;
	}
	dtms = scan.timeStamp - scan_prev.timeStamp;
	scan_prev.timeStamp = scan.timeStamp;
	scan_prev.nbPoints = scan.nbPoints;

  first_exec = false;

}